#ifndef _EXPL_DATA_H_
#define _EXPL_DATA_H_

#include <Eigen/Eigen>
#include <vector>
#include <bspline/Bspline.h>

using Eigen::Vector3d;
using std::vector;

namespace fast_planner {
/*
  FSM 数据：
      trigger_: 可能表示是否触发状态机的某个状态或行为（例如是否需要执行某个特定任务或规划）。
      have_odom_: 一个布尔值，表示是否获取到了里程计数据（可能用于确定机器人的位置和速度）。
      static_state_: 可能表示当前状态机是否处于静止状态，或者机器人是否处于静止状态。
      state_str_: 一个字符串向量，用于存储当前的状态描述（可能是状态机的各个状态名称或描述信息）。

  里程计数据（odom_*）：
      odom_pos_: 机器人的当前位置，使用 Eigen::Vector3d 表示三维空间中的位置。
      odom_vel_: 机器人的速度，使用 Eigen::Vector3d 表示三维空间中的速度。
      odom_orient_: 机器人的方向（姿态），使用 Eigen::Quaterniond 表示四元数形式的方向。
      odom_yaw_: 机器人的偏航角（yaw），通常用于表示机器人朝向的角度。

  初始状态（start_*）：
      start_pt_: 机器人的初始位置，使用 Eigen::Vector3d 表示。
      start_vel_: 机器人的初始速度，使用 Eigen::Vector3d 表示。
      start_acc_: 机器人的初始加速度，使用 Eigen::Vector3d 表示。
      start_yaw_: 机器人的初始偏航角。
      start_poss: 存储多个初始位置的向量，可能表示机器人的启动位置集合或某些控制点。

  轨迹数据（newest_traj_）：
      newest_traj_: 机器人的最新轨迹，使用 bspline::Bspline 类型表示，Bspline
  是一种常见的用于表示平滑轨迹的数学工具。

  群体避障数据：
      avoid_collision_: 布尔值，表示是否启用群体避障机制。
      go_back_: 布尔值，表示是否需要回退到某个位置（例如当机器人碰到障碍物时需要回退）。
      fsm_init_time_: ROS 时间类型，表示状态机初始化的时间。
      last_check_frontier_time_: ROS
  时间类型，表示上次检查前沿（frontier）点的时间，可能与探索任务有关。

  其他信息：
      start_pos_: 机器人的起始位置，类型为 Eigen::Vector3d，可能表示机器人从哪里开始执行任务。
*/
struct FSMData {
  // FSM data
  bool trigger_, have_odom_, static_state_;
  vector<string> state_str_;

  Eigen::Vector3d odom_pos_, odom_vel_;  // odometry state
  Eigen::Quaterniond odom_orient_;
  double odom_yaw_;

  Eigen::Vector3d start_pt_, start_vel_, start_acc_, start_yaw_;  // start state
  vector<Eigen::Vector3d> start_poss;
  bspline::Bspline newest_traj_;

  // Swarm collision avoidance
  bool avoid_collision_, go_back_;
  ros::Time fsm_init_time_;
  ros::Time last_check_frontier_time_;

  Eigen::Vector3d start_pos_;
  Eigen::Vector3d end_pos_;
};

/*
  FSMParam
  结构体主要用于存储与状态机相关的参数，用于调整机器人执行任务的行为，如重新规划的阈值、重新规划的时间、群体控制的参数等。

  重新规划阈值（replan_thresh1_, replan_thresh2_,
  replan_thresh3_）：这些是三个不同的阈值，用于触发不同阶段的重新规划（replanning）。
                      例如，当机器人在执行某个任务时，如果检测到某些条件（如位置偏离、速度变化等）超过这些阈值，可能会触发重新规划路径。

  重新规划时间（replan_time_）：这个参数表示重新规划的时间间隔，单位为秒。当需要重新规划路径或任务时，replan_time_
  控制重新规划的时间周期。 attempt_interval_: 最小优化尝试间隔，表示两次优化尝试之间的最小时间间隔。
  pair_opt_interval_: 成功配对优化之间的最小时间间隔，可能与多机器人系统中的协作优化相关。
  repeat_send_num_:
  表示重复发送控制指令的次数，可能用于确保任务的可靠执行（例如，多次尝试以确保指令被正确接收）。
*/
struct FSMParam {
  double replan_thresh1_;
  double replan_thresh2_;
  double replan_thresh3_;
  double replan_time_;  // second

  // Swarm
  double attempt_interval_;   // Min interval of opt attempt
  double pair_opt_interval_;  // Min interval of successful pair opt
  int repeat_send_num_;
};

/*
  DroneState
  结构体看起来是用于存储关于单个无人机（Drone）的状态信息。它包括了无人机的位置、速度、航向角等运动状态，以及与其他无人机相关的交互和优化尝试的信息。以下是对该结构体各成员的详细分析：

  结构体成员变量分析：
  1. Eigen::Vector3d pos_;
  表示无人机的位置，使用 Eigen::Vector3d
  类型表示三维空间中的位置。这个成员变量用于存储无人机在空间中的 X、Y、Z 坐标。
  2. Eigen::Vector3d vel_;
  表示无人机的速度，同样使用 Eigen::Vector3d
  类型表示三维速度向量。这个成员变量用于存储无人机在空间中沿 X、Y、Z 轴的速度分量。
  3. double yaw_;
  表示无人机的偏航角（Yaw），通常用于描述无人机朝向的角度。偏航角是一个标量值，通常以弧度或度为单位。它描述了无人机相对于某个参考方向的旋转角度，常用于计算无人机的航向。
  4. double stamp_;
  时间戳，表示该位置、速度和航向角数据的时间，通常是相对于某个起始时间的时间戳（以秒为单位）。这用于跟踪数据的时效性和同步。
  例如，如果系统中存在多个传感器或多个数据来源，stamp_
  有助于确保在处理这些数据时能够按正确的时间顺序对数据进行处理。
  5. double recent_attempt_time_;
  记录最近一次与任何无人机进行优化尝试的时间戳。这可能用于协调多个无人机之间的优化任务或避免同时进行多个优化尝试而产生冲突。
  这个变量的作用可能是帮助确定当前无人机是否与其他无人机进行过合作或交互，例如在多机器人任务中，某些任务可能需要多个无人机的协作。
  6. vector<int> grid_ids_;
  存储当前无人机在网格巡航中涉及到的网格编号（grid_ids_）。这是一个整数向量，可能表示无人机在执行巡航任务时所访问的不同网格区域的
  ID。
  在某些任务中（例如探索任务、区域扫描等），无人机可能被划分到不同的网格，每个网格可能有一个唯一的
  ID，通过 grid_ids_ 可以追踪无人机所覆盖的区域。
  7. double recent_interact_time_;
  记录最近一次与其他无人机进行交互的时间戳。交互可能包括信息交换、协作任务、避障等。
  该变量用于跟踪无人机之间的交互时间，确保协作过程的协调性。它可能用于避免不同无人机之间的竞争或冲突，例如在多无人机协作时，确保无人机之间不会频繁或无序地进行交互。
*/

struct DroneState {
  Eigen::Vector3d pos_;
  Eigen::Vector3d vel_;
  double yaw_;
  double stamp_;                // Stamp of pos,vel,yaw
  double recent_attempt_time_;  // Stamp of latest opt attempt with any drone

  vector<int> grid_ids_;         // Id of grid tour
  double recent_interact_time_;  // Stamp of latest opt with this drone
  Eigen::Vector3d next_pos_;
  Eigen::Vector3d pre_pos_;
  double pre_time_;
};

/*
  ExplorationData
  结构体是一个复杂的数据容器，包含了与探索和覆盖规划相关的各种信息，特别是与无人机（或者多无人机群体）进行环境探索和路径规划的过程密切相关。
  它包括了前沿（frontiers）、路径（paths）、无人机状态（swarm_state）等数据，以及一些用于优化和协调群体行为的信息。

  结构体成员变量分析
  1. 前沿（Frontiers）相关数据：
  frontiers_: 这个变量是一个二维
  vector，存储多个前沿点（Vector3d）。前沿点通常是探索中的“边界”区域，代表机器人在探索时尚未覆盖的空间区域。
  dead_frontiers_: 与 frontiers_
  相对应，存储的是“死前沿”（不再需要探索的前沿点），这些点可能是已经被覆盖，或者因为其他原因（如障碍物、传感器问题等）变得不可达。
  frontier_boxes_: 存储了一系列前沿区域的“盒子”，每个“盒子”由两个 Vector3d
  对象表示，可能用于定义前沿区域的边界（如长方体框）。 frontier_tour_:
  存储探索过程中依次访问的前沿点，可能代表一个按顺序访问的路径（或者是访问某些特定区域的顺序）。
  other_tours_:
  存储其他无人机的探索路径，这对于多无人机群体协作时的路径规划非常重要，可以避免路径冲突和重复覆盖。
  2. 位置、路径和方向：
  points_: 存储某些重要的点，可能是计划中的路径上的关键位置或者前沿点。
  averages_: 存储平均位置或其他计算得出的平均值（可能与环境、机器人状态等有关）。
  views_: 存储视角点，用于规划无人机的视角方向或者图像传感器的朝向。
  yaws_: 存储机器人在对应位置的偏航角（yaw）。这个信息通常与机器人的朝向或视角相关。
  3. 规划和路径：
  path_next_goal_: 存储前往下一个目标的路径。
  kino_path_: 存储基于运动学模型的路径（可能是为了避免碰撞或者进行更精细的路径规划）。
  next_goal_: 存储下一个目标点的位置信息，通常在路径规划和任务规划中用到。
  next_pos_ 和 next_yaw_: 存储下一个目标位置和目标的偏航角。
  4. 视角规划：
  views_vis1_ 和 views_vis2_:
  存储规划的两个视角方向点集合，可能代表了无人机在不同视角下的观测区域或任务执行过程中的视角。
  centers_ 和 scales_: 存储视角规划中所需的中心点和缩放尺度，用于控制机器人视角的范围或影响区域。
  5. 群体行为和其他无人机状态（Swarm）：
  swarm_state_: 存储与其他无人机的状态信息，使用 DroneState 类型表示。每个 DroneState
  包含无人机的状态数据（如位置、速度、航向、交互时间等）。这对于多无人机协作尤为重要。
  pair_opt_stamps_ 和 pair_opt_res_stamps_:
  存储成对优化操作的时间戳，可能是用于同步优化过程或记录与其他无人机的优化过程。 ego_ids_ 和
  other_ids_: 存储无人机自身和其他无人机的标识符，用于区分不同的无人机。 pair_opt_stamp_:
  存储最后一次优化操作的时间戳。 reallocated_ 和 wait_response_:
  这两个布尔变量表示是否已经重新分配任务和是否在等待响应。可能用于在多无人机协作中，等待某些操作的完成或协调。
  6. 覆盖规划：
  grid_tour_ 和 grid_tour2_:
  存储网格覆盖路径，通常用于确保环境被覆盖并且没有遗漏的区域。这些路径可能是规划的机器人的移动路径。
  last_grid_ids_: 存储上一次网格覆盖过程中的网格标识符，用于跟踪哪些区域已经覆盖。
  7. 任务和计划：
  plan_num_: 存储当前计划的编号或计数，用于区分不同的任务或规划。
*/

struct ExplorationData {
  vector<vector<Vector3d>> frontiers_;
  vector<vector<Vector3d>> dead_frontiers_;
  vector<pair<Vector3d, Vector3d>> frontier_boxes_;
  vector<Vector3d> points_;
  vector<Vector3d> averages_;
  vector<Vector3d> views_;
  vector<double> yaws_;
  vector<Vector3d> frontier_tour_;
  vector<vector<Vector3d>> other_tours_;

  vector<int> refined_ids_;
  vector<vector<Vector3d>> n_points_;
  vector<Vector3d> unrefined_points_;
  vector<Vector3d> refined_points_;
  vector<Vector3d> refined_views_;  // points + dir(yaw)
  vector<Vector3d> refined_views1_, refined_views2_;
  vector<Vector3d> refined_tour_;

  Vector3d next_goal_;
  vector<Vector3d> path_next_goal_, kino_path_;
  Vector3d next_pos_;
  double next_yaw_;

  // viewpoint planning
  // vector<Vector4d> views_;
  vector<Vector3d> views_vis1_, views_vis2_;
  vector<Vector3d> centers_, scales_;

  // Swarm, other drones' state
  vector<DroneState> swarm_state_;
  vector<double> pair_opt_stamps_, pair_opt_res_stamps_;
  vector<int> ego_ids_, other_ids_;
  double pair_opt_stamp_;
  bool reallocated_, wait_response_;

  // Coverage planning
  vector<Vector3d> grid_tour_, grid_tour2_;
  // int prev_first_id_;
  vector<int> last_grid_ids_;

  int plan_num_;
  bool collision = false;
  vector<Vector3d> collision_pos_;
};

/*
  ExplorationParam
  结构体主要用于存储与探索过程相关的参数配置，包括无人机的行为、路径规划、群体行为和协作等方面的参数。这些参数决定了无人机群体在执行任务时的具体策略和算法配置。

  结构体成员变量分析
  1. 参数相关配置：
  refine_local_: 一个布尔值，表示是否进行局部优化（refinement）。如果为
  true，可能表示在探索过程中某些区域会进行精细化的进一步探索或优化。 refined_num_:
  一个整数，表示进行精细化优化时使用的区域数量。可能是指精细化优化的前沿点数量或路径数量。
  refined_radius_: 一个浮动值，表示精细化优化区域的半径，可能用于限定优化区域的范围。例如，周围
  refined_radius_ 距离内的区域会进行更高精度的探索。 top_view_num_:
  整数值，表示在某些任务中选择的“最优视角”数量。这个参数可能用于选择最具代表性或最有效的视角，用于视觉规划或者多视角分析。
  max_decay_:
  一个浮动值，表示某些信息的最大衰减值。比如在路径规划或覆盖率规划中，如果某些区域长时间未被访问，可能需要对这些区域的权重进行衰减。max_decay_
  控制这种衰减的最大程度。 tsp_dir_ 和 mtsp_dir_: 分别是 TSP（旅行商问题，Traveling Salesman
  Problem）和 MTSP（多旅行商问题，Multiple Traveling Salesman
  Problem）求解器的资源目录路径。这些参数指定了用于解决 TSP 或 MTSP
  问题的算法或求解器的路径，可能是外部程序或库的路径。 relax_time_:
  一个浮动值，表示任务中的“放松时间”。可能与路径规划或任务调度中的松弛时间（例如，任务延迟或等待）有关。
  init_plan_num_:
  一个整数，表示初始化时的计划数量。可能是初始探索过程中待执行的任务或路径规划的数量。
  2. 群体行为相关配置：
  drone_num_:
  表示当前任务或系统中无人机的数量。这个参数决定了参与探索的无人机的数量，适用于群体协调、路径规划等方面的计算。
  drone_id_:
  当前无人机的标识符。这个参数用于标识当前无人机在群体中的编号或ID，帮助区分不同的无人机，以便进行个性化控制或协调任务。
*/

struct ExplorationParam {
  // params
  bool refine_local_;
  int refined_num_;
  double refined_radius_;
  int top_view_num_;
  double max_decay_;
  string tsp_dir_;   // resource dir of tsp solver
  string mtsp_dir_;  // resource dir of tsp solver
  double relax_time_;
  int init_plan_num_;

  // Swarm
  int drone_num_;
  int drone_id_;
};

}  // namespace fast_planner

#endif